/**
 * Copyright (c) The openTCS Authors.
 * <p>
 * This program is free software and subject to the MIT license. (For details,
 * see the licensing information (LICENSE.txt) you should have received with
 * this copy of the software.)
 */
package zdh.fsy.jjwcVehicle;

import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.google.inject.assistedinject.Assisted;
import jakarta.inject.Inject;

import java.beans.PropertyChangeEvent;
import java.io.IOException;
import java.util.*;

import static java.util.Objects.isNull;
import static java.util.Objects.requireNonNull;

import java.util.concurrent.*;
import java.util.stream.Collectors;

import jjwc_fsy.HttpManager;
import org.opentcs.common.JJWCLoopAdapterConstants;
import org.opentcs.customizations.kernel.KernelExecutor;
import org.opentcs.data.model.Path;
import org.opentcs.data.model.Point;
import org.opentcs.data.model.Vehicle;
import org.opentcs.data.order.Route.Step;
import org.opentcs.data.order.TransportOrder;
import org.opentcs.drivers.vehicle.*;
import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO;
import org.opentcs.util.ExplainedBoolean;
import org.opentcs.util.fsy.JsonBinder;
import org.opentcs.util.fsy.SourceUtils;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import zdh.fsy.jjwcVehicle.rxx.processmsg.RXXVehicleData;
import zdh.fsy.jjwcVehicle.rxx.processmsg.VehicleStateData;
import zdh.fsy.jjwcVehicle.rxx.sendmsg.SendPathCmd;

/**
 * A {@link VehicleCommAdapter} that does not really communicate with a physical vehicle but roughly
 * simulates one.
 */
public class JJWCLoopCommunicationAdapter extends BasicVehicleCommAdapter implements SimVehicleCommAdapter {

  /**
   * The name of the load handling device set by this adapter.
   */
  public static final String LHD_NAME = "default";
  /**
   * This class's Logger.
   */
  private static final Logger LOG = LoggerFactory.getLogger(JJWCLoopCommunicationAdapter.class);
  /**
   * An error code indicating that there's a conflict between a load operation and the vehicle's
   * current load state.
   */
  private static final String LOAD_OPERATION_CONFLICT = "cannotLoadWhenLoaded";
  /**
   * An error code indicating that there's a conflict between an unload operation and the vehicle's
   * current load state.
   */
  private static final String UNLOAD_OPERATION_CONFLICT = "cannotUnloadWhenNotLoaded";
  /**
   * The time (in ms) of a single simulation step.
   */
  private static final int SIMULATION_PERIOD = 100;
  /**
   * This instance's configuration.
   */
  private final JJWCLoopVehicleConfiguration configuration;
  /**
   * Indicates whether the vehicle simulation is running or not.
   */
  private volatile boolean isSimulationRunning;
  /**
   * The vehicle to this comm adapter instance.
   */
  private final Vehicle vehicle;
  /**
   * The vehicle's load state.
   */
  private LoadState loadState = LoadState.EMPTY;
  /**
   * Whether the JJWCLoop adapter is initialized or not.
   */
  private boolean initialized;
  /**
   * 定时任务的对象
   */
  private ScheduledExecutorService vehicle_state_task;
  /**
   * 执行的定时拉去车辆状态的定时任务
   */
  private ScheduledFuture<?> scheduledFuture;

  private JsonBinder jsonBinder = new JsonBinder();
  /**
   * 发送连接的请求次数（在连接不上的情况下）
   */
  private Integer sendConnectTime = 0;

  /**
   * 这是最新的小车状态
   */
  private Vehicle.State currentState;
  /**
   * 这是小车最新的处理订单状态
   */
  private Vehicle.ProcState currentProcState;
  /**
   * 这是表明车辆是否在路径中间，不在站点上。通过状态查询进行更新
   */
  private boolean isInPath = false;
  /**
   * 小车请求的端口号
   */
  private final Integer vehiclePort = 50001;

  /**
   * Creates a new instance.
   *
   * @param configuration  This class's configuration.
   * @param vehicle        The vehicle this adapter is associated with.
   * @param kernelExecutor The kernel's executor.
   */
  @Inject
  public JJWCLoopCommunicationAdapter(JJWCLoopVehicleConfiguration configuration, @Assisted Vehicle vehicle, @KernelExecutor ScheduledExecutorService kernelExecutor) {
    super(new JJWCLoopVehicleModel(vehicle), configuration.commandQueueCapacity(), configuration.rechargeOperation(), kernelExecutor);
    this.vehicle = requireNonNull(vehicle, "vehicle");
    this.configuration = requireNonNull(configuration, "configuration");
  }

  @Override
  public void initialize() {
    if (isInitialized()) {
      return;
    }
    super.initialize();

    String initialPos = vehicle.getProperties().get(JJWCLoopAdapterConstants.PROPKEY_INITIAL_POSITION);
    if (initialPos != null) {
      initVehiclePosition(initialPos);
    }
    getProcessModel().setState(Vehicle.State.IDLE);
    getProcessModel().setLoadHandlingDevices(Arrays.asList(new LoadHandlingDevice(LHD_NAME, false)));
    initialized = true;
  }

  @Override
  public boolean isInitialized() {
    return initialized;
  }

  @Override
  public void terminate() {
    if (!isInitialized()) {
      return;
    }
    super.terminate();
    initialized = false;
  }

  @Override
  public void propertyChange(PropertyChangeEvent evt) {
    super.propertyChange(evt);

    if (!((evt.getSource()) instanceof JJWCLoopVehicleModel)) {
      return;
    }
    if (Objects.equals(evt.getPropertyName(), VehicleProcessModel.Attribute.LOAD_HANDLING_DEVICES.name())) {
      if (!getProcessModel().getLoadHandlingDevices().isEmpty() && getProcessModel().getLoadHandlingDevices().get(0).isFull()) {
        loadState = LoadState.FULL;
        getProcessModel().setLength(configuration.vehicleLengthLoaded());
      } else {
        loadState = LoadState.EMPTY;
        getProcessModel().setLength(configuration.vehicleLengthUnloaded());
      }
    }
    if (Objects.equals(evt.getPropertyName(), JJWCLoopVehicleModel.Attribute.SINGLE_STEP_MODE.name())) {
      // When switching from single step mode to automatic mode and there are commands to be
      // processed, ensure that we start/continue processing them.
      if (!getProcessModel().isSingleStepModeEnabled() && !getSentCommands().isEmpty() && !isSimulationRunning) {
        isSimulationRunning = true;
//        ((ExecutorService) getExecutor()).submit(() -> startVehicleSimulation(getSentCommands().peek()));
      }
    }
  }

  @Override
  public synchronized void enable() {
    if (isEnabled()) {
      return;
    }
    //这里需要开启一个定时任务处理请求小车的状态
    if (vehicle_state_task != null) {
      vehicle_state_task = null;
    }
    vehicle_state_task = Executors.newSingleThreadScheduledExecutor();
    scheduledFuture = vehicle_state_task.scheduleAtFixedRate(getVehicleStateTask(), 100, 200, TimeUnit.MILLISECONDS);
    super.enable();
  }

  /**
   * 定时任务获取小车的状态
   * 该定时任务会在0.2秒的周期内定时发送http请求获取小车的实时状态。如果请求失败会进行一次长达(请求失败次数*500)ms睡眠，
   * 当连续请求失败达到6次还为请求成功，会直接将任务进行取消，这时候需要人工进行重新enable
   *
   * @return
   */
  private Runnable getVehicleStateTask() {
    return new Runnable() {
      @Override
      public void run() {
        String vehicleIp = getProcessModel().getVehicleIp();
        if (vehicleIp == null) {
          LOG.error("vehicle ip is null,can not send http request");
          scheduledFuture.cancel(true);
          return;
        }
        //这里最多进行5此尝试连接，如果配置文件配置的低于5次那么就用配置文件配置的次数 结果就是Min(5,配置文件的次数)
        for (int i = 1; i < 6; i++) {
          try {
            String requestData = HttpManager.sendRequest(vehicleIp, vehiclePort, "/rbrainrobot/get_robot_run_status", getName(), true);
//          LOG.info(requestData);
            RXXVehicleData rxxVehicleStateData = jsonBinder.fromJson(requestData, RXXVehicleData.class);
            processMessage(rxxVehicleStateData.getVehicleStateData());
          } catch (Exception e) {
            //如果断链的情况下 就会根据尝试的次数进行定时睡眠；
            if (i >= configuration.relayConnectTime()) {
              //取消定时任务
              disable();
              return;
            }
            try {
              LOG.warn("request vehicle state failed,sleep [{}]ms", i * 500);
              TimeUnit.MILLISECONDS.sleep(500L * i);
            } catch (InterruptedException ex) {
              throw new RuntimeException(ex);
            }
            LOG.info("search vehicle state exception,error_msg:{}", e.getMessage());
            setVehicleState(Vehicle.State.UNKNOWN);
            continue;
          }
          break;
        }
      }
    };
  }

  @Override
  public synchronized void disable() {
    if (!isEnabled()) {
      return;
    }
    //这里关闭定时发送查询小车状态的定时任务
    if (!scheduledFuture.isCancelled()) {
      scheduledFuture.cancel(true);
    }
    scheduledFuture = null;
    vehicle_state_task.shutdownNow();
    vehicle_state_task = null;
    //关闭之后，需要将状态修改为断开的模式
    setVehicleState(Vehicle.State.UNKNOWN);
    super.disable();
  }

  @Override
  public JJWCLoopVehicleModel getProcessModel() {
    return (JJWCLoopVehicleModel) super.getProcessModel();
  }

  @Override
  public synchronized void sendCommand(MovementCommand cmd) {
    requireNonNull(cmd, "cmd");
    Point sourcePoint = cmd.getStep().getSourcePoint();
    String start_node = sourcePoint == null ? getProcessModel().getPosition() : sourcePoint.getName();
    if (sourcePoint == null) {
      LOG.warn("do local action");
    }
    Point destinationPoint = cmd.getStep().getDestinationPoint();
    Integer cmdId = cmd.getCmdId();
    String mapName = vehicle.getProperty(Vehicle.MAP_PROPERTY);
    String gold_node = destinationPoint.getName();

    //是否需要进行倒退
    Boolean reverse = null;
    if (cmd.getStep().getPath() != null && cmd.getStep().getPath().getProperty("reverse") != null) {
      reverse = Boolean.valueOf(cmd.getStep().getPath().getProperty("reverse"));
    }
    SendPathCmd.PathCmdData pathCmdData = new SendPathCmd.PathCmdData(mapName, start_node, gold_node, cmdId, null, isInPath, reverse);
    SendPathCmd sendPathCmd = new SendPathCmd(pathCmdData);
    String sendCmdMsg = sendPathCmd.getSendCmdMsg();
    LOG.info("send vehicle path cmd:{},", sendCmdMsg);
    String vehicleIp = getProcessModel().getVehicleIp();
    boolean sendSuccess = false;
    try {
      sendSuccess = requestIsSuccess(HttpManager.sendRequestByPost(vehicleIp, vehiclePort, sendPathCmd.getRequestPath(), sendCmdMsg, getName()));
    } catch (IOException e) {
      LOG.warn("{} send path cmd failed,error_msg:{}", getName(), e.getMessage());
    }
    if (sendSuccess) {
      //todo:将发送的数据进行保存？
      LOG.info("send path cmd to vehicle: {} ip:{}", getName(), vehicleIp);
    } else {
      //todo:发送失败进行重发；
      if (!isEnabled()) {
        LOG.warn("{} is disabled,can not send http request", getName());
      }
    }
  }

  @Override
  public void onVehiclePaused(boolean paused) {
    getProcessModel().setVehiclePaused(paused);
  }

  @Override
  public void processMessage(Object message) {
    if (!(message instanceof VehicleStateData)) {
      LOG.warn("message data that can not be processed by {}", getName());
      return;
    }
    VehicleStateData data = (VehicleStateData) message;
//    LOG.info("process a vehicleState message:{}", data);
    String currentPosition = data.getCurrentPosition();
    if (currentPosition != null && !currentPosition.isEmpty()) {
      getProcessModel().setPosition(currentPosition);
    }
    //更新小车的置信度
    Float locationScore = data.getLocation_score();
    getProcessModel().setProperty(Vehicle.ScoreKey, String.format("%.2f", (locationScore * 100)));
    //更新小车是否处于路径上
    List<String> robotInPath = data.getRobot_in_path();
    isInPath = robotInPath != null && !robotInPath.isEmpty();
    //处理资源释放
    List<Integer> finishPathIds = data.getFinishPathIds();
    if (finishPathIds != null && !finishPathIds.isEmpty()) {
      Queue<MovementCommand> sentCommands = getSentCommands();
      List<MovementCommand> collect = sentCommands.stream().filter(cmds -> {
        int routeIndex = cmds.getCmdId();
        return finishPathIds.contains(routeIndex);
      }).collect(Collectors.toList());
      for (MovementCommand movementCommand : collect) {
        //todo:
        MovementCommand currentcommand =getSentCommands().peek();
        if (!currentState.equals(Vehicle.State.WAITING)&&isVehicleWaiting(currentcommand)){
                setVehicleState(Vehicle.State.WAITING);

              }
        getProcessModel().commandExecuted(movementCommand);
        getSentCommands().remove(movementCommand);
        LOG.info("{} success process cmdId:{} pathSrc:{} pathDes:{}", getName(), movementCommand.getCmdId(), movementCommand.getStep().getSourcePoint(), movementCommand.getStep().getDestinationPoint());
      }



    }
    //处理小车状态

    //todo:这里有tip
    if (currentProcState.equals(Vehicle.ProcState.IDLE)&&!currentState.equals(Vehicle.State.WAITING)) {
      setVehicleState(Vehicle.State.IDLE);
    } else if (!currentState.equals(Vehicle.State.ERROR)) {
      if (data.vehicleIsCharging()) {
        setVehicleState(Vehicle.State.CHARGING);
      } else if(!currentState.equals(Vehicle.State.WAITING)){
        setVehicleState(Vehicle.State.EXECUTING);
      }
    } else {
      LOG.warn("{} is in error state", getName());
      setVehicleState(Vehicle.State.ERROR);
    }
  }

  private boolean isVehicleWaiting(MovementCommand command) {
    // 判断条件：command 不是 null, 是 final movement, 且 final movement 不是 PARK,
    return command != null
        && command.isFinalMovement()
        && !command.getFinalOperation().equalsIgnoreCase("PARK");
  }




  @Override
  public void updateVehicleStateFromKernel(Vehicle currVehicleState) {
    currentState = currVehicleState.getState();
    currentProcState = currVehicleState.getProcState();
  }

  @Override
  public synchronized void initVehiclePosition(String newPos) {
    ((ExecutorService) getExecutor()).submit(() -> getProcessModel().setPosition(newPos));
  }

  @Override
  public synchronized ExplainedBoolean canProcess(TransportOrder order) {
    requireNonNull(order, "order");

    return canProcess(order.getFutureDriveOrders().stream().map(driveOrder -> driveOrder.getDestination().getOperation()).collect(Collectors.toList()));
  }

  @Override
  @Deprecated
  public synchronized ExplainedBoolean canProcess(List<String> operations) {
    requireNonNull(operations, "operations");

    LOG.debug("{}: Checking processability of {}...", getName(), operations);
    boolean canProcess = true;
    String reason = "";

    // Do NOT require the vehicle to be IDLE or CHARGING here!
    // That would mean a vehicle moving to a parking position or recharging location would always
    // have to finish that order first, which would render a transport order's dispensable flag
    // useless.
    boolean loaded = loadState == LoadState.FULL;
    Iterator<String> opIter = operations.iterator();
    while (canProcess && opIter.hasNext()) {
      final String nextOp = opIter.next();
      // If we're loaded, we cannot load another piece, but could unload.
      if (loaded) {
        if (nextOp.startsWith(getProcessModel().getLoadOperation())) {
          canProcess = false;
          reason = LOAD_OPERATION_CONFLICT;
        } else if (nextOp.startsWith(getProcessModel().getUnloadOperation())) {
          loaded = false;
        }
      } // If we're not loaded, we could load, but not unload.
      else if (nextOp.startsWith(getProcessModel().getLoadOperation())) {
        loaded = true;
      } else if (nextOp.startsWith(getProcessModel().getUnloadOperation())) {
        canProcess = false;
        reason = UNLOAD_OPERATION_CONFLICT;
      }
    }
    if (!canProcess) {
      LOG.debug("{}: Cannot process {}, reason: '{}'", getName(), operations, reason);
    }
    return new ExplainedBoolean(canProcess, reason);
  }

  @Override
  protected synchronized void connectVehicle() {
    boolean done = scheduledFuture.isDone();
  }

  @Override
  protected synchronized void disconnectVehicle() {
  }

  @Override
  protected synchronized boolean isVehicleConnected() {
    return true;
  }

  @Override
  protected VehicleProcessModelTO createCustomTransferableProcessModel() {
    return new JJWCLoopVehicleModelTO().setLoadOperation(getProcessModel().getLoadOperation()).setMaxAcceleration(getProcessModel().getMaxAcceleration()).setMaxDeceleration(getProcessModel().getMaxDecceleration()).setMaxFwdVelocity(getProcessModel().getMaxFwdVelocity()).setMaxRevVelocity(getProcessModel().getMaxRevVelocity()).setOperatingTime(getProcessModel().getOperatingTime()).setSingleStepModeEnabled(getProcessModel().isSingleStepModeEnabled()).setUnloadOperation(getProcessModel().getUnloadOperation()).setVehiclePaused(getProcessModel().isVehiclePaused());
  }

  /**
   * modify the vehicle state
   *
   * @param state: the new state
   */
  private void setVehicleState(Vehicle.State state) {
    if (!getProcessModel().getState().equals(state)) {
      getProcessModel().setState(state);
      LOG.info("Vehicle state changed: {}", state);
    }
  }

  /**
   * The vehicle's possible load states.
   */
  private enum LoadState {
    EMPTY, FULL;
  }

  private boolean requestIsSuccess(String jsonData) {
    if (jsonData == null || jsonData.isEmpty()) {
      return false;
    }
    ObjectMapper mapper = new ObjectMapper();
    try {
      JsonNode jsonNode = mapper.readTree(jsonData);
      String msg = jsonNode.get("data").get("msg").toString();
      LOG.info("send request rely msg data: {}", msg);
      return "ok".equals(msg);
    } catch (JsonProcessingException e) {
      LOG.warn("Failed to parse json data: {}", jsonData);
      return false;
    }
  }
}
